#include "text_video.h"
#include "stdint.h"
#include "stdio.h"
#include "string.h"

static uint8_t x, y;
static uint16_t color;

static void scroll(void);
static uint32_t print_int(const uint32_t i);
static uint32_t print_hex(const uint32_t i);

void clear_text_video()
{
    int i;
    
    /* zero the screen */
    memclr((uint8_t *)TEXT_VIDEO_BASE, TEXT_VIDEO_SIZE);
    
    x = 0;
    y = TEXT_VIDEO_HEIGHT - 1;
}

void init_text_video()
{   
    x = y = 0;
    
    set_text_color(default_color);
    set_background_color(0x00);
    clear_text_video();
}

void set_text_color(const uint8_t col) 
{
    color = (uint16_t)((color & 0xF000) | (((uint16_t)col << 8) & 0x0F00));
}

void set_background_color(const uint8_t col)
{
    color = (uint16_t)((color & 0x0F00) | (((uint16_t)col << 12) & 0xF000));
}

static void scroll(void)
{
    /* move everything up one line, and zero the new bottom line */
    memcpy((uint8_t *)TEXT_VIDEO_BASE, (uint8_t *)(TEXT_VIDEO_BASE + (TEXT_VIDEO_WIDTH * 2)), TEXT_VIDEO_SIZE - (TEXT_VIDEO_WIDTH * 2));
    memclr((uint8_t *)TEXT_VIDEO_BASE + TEXT_VIDEO_SIZE - (TEXT_VIDEO_WIDTH * 2), TEXT_VIDEO_WIDTH * 2);

    x = 0;
    y = TEXT_VIDEO_HEIGHT - 1;
}

/* no calling putch(x) directly, use printf or siblings */
static void putch(const char c)
{
    /* if it's a newline, increase Y position and reset X position. */
    if(c == '\n')           
    {
        x = 0;
        y++;
        
        /* if we've gone past the bottom of the screen, it's time to scroll */
        if(y >= TEXT_VIDEO_HEIGHT)         
        {scroll();}
        return;
    }
    else if(c == '\b')
    {
        if(!x){y--; x = 80-1;}
        else{x--;}
        
        return;
    }
    

    *(uint16_t*)(TEXT_VIDEO_BASE + ((y * 160) + (x++ * 2))) = (uint16_t)(color | c);
    
    if(x >= 80)
    {
        x = 0;
        y++;
        
        if(y >= TEXT_VIDEO_HEIGHT)
        {scroll();}
        return;
    }
}

static uint32_t print_int(const uint32_t i)
{
    uint32_t n = 0;
    uint32_t d = 1000000000;
    uint32_t count = 0;
    
    while((i / d == 0) && (d >= 10))
    {d /= 10;}
  
    n = i;
  
    while(d >= 10)
    {
        count++;
        putch((char)('0' + n / d));
        n = n % d;
        d /= 10;
    }
    count++;
    putch((char)('0' + n));
    return count;
}   

static uint32_t print_hex(const uint32_t i)
{
    const uint8_t hex[16] = { '0', '1', '2', '3', '4', '5', '6', '7',
                              '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' };
  
    uint32_t n = 0;
    uint32_t d = 0x10000000;
    uint32_t count = 0;
    
    while((i / d == 0) && (d >= 0x10))
    {
        d /= 0x10;
    }
    
    n = i;
    
    if(d < 0x10){putch('0');}
    while(d >= 0xF)
    {
        count++;
        putch((char)(hex[n / d]));
        n = n % d;
        d /= 0x10;
    }
    count++;
    putch((char)(hex[n]));
    return count;
}

void printf(const char * const buf, ...)
{
    uint32_t pos, count;
    uint8_t * str;

    va_list ap;
    va_start(ap, buf);
    
    pos = 0;
    count = 0;
    
    while(buf[pos])
    {
        if(buf[pos] == '%')
        {
            pos++;
            switch(buf[pos])
            {
              case 'c':
                  putch((char)va_arg(ap, int));
                  count++;
                  
                  break;
              case 's':
                  str = va_arg(ap, uint8_t*);
                  while(1)
                  {
                      count++;
                      putch(*(char*)str++);
                      if(!(*(uint8_t*)str)){break;}
                  }
                  break;

              case 'd':
              case 'u':  
                  count += print_int(va_arg(ap, uint32_t)); 
                  break;
              case 'x':   
                  putch('0');
                  putch('x');
                  count += print_hex(va_arg(ap, uint32_t)) + 2;
                  break;
              case 'm':
                  count += print_hex(va_arg(ap, uint32_t)) + 2;
                  break;
              default:
                  putch((char)buf[pos]);
                  count++;
            }
            pos++;
        }
        else
        {
          putch((char)buf[pos]);
          count++;
          pos++;
        }
    }
    va_end(ap);
}

int vfprintf ( uint8_t * stream, const char * buf, va_list ap )
{
    int pos, count;
    uint8_t * str;
    
    pos = 0;
    count = 0;
    
    while(buf[pos])
    {
        if(buf[pos] == '%')
        {
            pos++;
            switch(buf[pos])
            {
              case 'c':
                  putch((char)va_arg(ap, int));
                  count++;
                  
                  break;
              case 's':
                  str = va_arg(ap, uint8_t*);
                  while(1)
                  {
                      count++;
                      putch(*(char*)str++);
                      if(!(*(uint8_t*)str)){break;}
                  }
                  break;

              case 'd':
              case 'u':  
                  count += print_int(va_arg(ap, uint32_t)); 
                  break;
              case 'x':   
                  putch('0');
                  putch('x');
                  count += print_hex(va_arg(ap, uint32_t)) + 2;
                  break;
              case 'm':
                  count += print_hex(va_arg(ap, uint32_t)) + 2;
                  break;
              default:
                  putch((char)buf[pos]);
                  count++;
            }
            pos++;
        }
        else
        {
          putch((char)buf[pos]);
          count++;
          pos++;
        }
    }
    va_end(ap);
    
    return count;
}




